import serial
import socket

Serv = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
Serv.bind(('', 9000))
Serv.listen(1)
print "Listening on TCP 9000"
motor = serial.Serial('/dev/ttyUSB0', 9600, timeout=1)
print "Connected to Motor Controller: /dev/ttyUSB0"

while(1):
	print "Waiting For Connection..."
	connection, addr = Serv.accept()
	connection.setblocking(0)
	print "Connected by", addr[0]
	while(1):
		try:
			Servdata = connection.recv(1)
			break
		except:
			pass
	if (Servdata == 'M'):
		print "Entering Manual Mode"
		motor.write(Servdata)
		while(Servdata != 'X'):
			Servdata = '9'
			try:
				Servdata = connection.recv(1)
			except:
				pass
			if Servdata == 'X':
				print "Exiting"
				break
			if Servdata == '0':
				print "Stopping"
				motor.write(Servdata)
			if Servdata == '1':
				print "Forward"
				motor.write(Servdata)
			if Servdata == '2':
				print "Left"
				motor.write(Servdata)
			if Servdata == '3':
				print "Right"
				motor.write(Servdata)
			if Servdata == '4':
				motor.write(Servdata)
				print "Backwards"
			if Servdata == '~':
				motor.write(Servdata)
				retval = motor.readline()
				Serv.send(retval)
			else:
				pass
		motor.write('0')
		connection.close()
		print addr[0], "Closed Manual Mode"
	
